#include "stdafx.h"
#include "Quaternion.h"
#include <cmath>

//-------------------------------------------------------------------------
// constructors
//-------------------------------------------------------------------------

CQuaternion::CQuaternion()
{
	w = 1.0f; // multiplication identity quaternion
}

CQuaternion::CQuaternion(float w_, float x_, float y_, float z_)
{
	w = w_;
	v.x = x_;
	v.y = y_;
	v.z = z_;
	Normalize();
}

CQuaternion::CQuaternion(float w_, vector3f v_)
{
	w = w_;
	v = v_;
	Normalize();
}

//-------------------------------------------------------------------------
// member functions
//-------------------------------------------------------------------------

void CQuaternion::Set(float w_, float x_, float y_, float z_)
{
	w = w_;
	v.x = x_;
	v.y = y_;
	v.z = z_;
	Normalize();
}

void CQuaternion::Set(float w_, vector3f v_)
{
	w = w_;
	v = v_;
	Normalize();
}

float CQuaternion::MagnitudeSquared()
{
	return w*w + v.x*v.x + v.y*v.y + v.z*v.z;
}

float CQuaternion::Magnitude()
{
	return sqrtf(w*w + v.x*v.x + v.y*v.y + v.z*v.z);
}

void CQuaternion::Normalize()
{
	float mag = Magnitude();
	if (mag == 0.0f) mag = 1.0f;
	float avoid_div = 1.0f / mag;
	w *= avoid_div;
	v.x *= avoid_div;
	v.y *= avoid_div;
	v.z *= avoid_div;
}

void CQuaternion::EulerAnglesToQuat(float x, float y, float z)
{
	float halfx = DEGTORAD(x) * 0.5f;
	float halfy = DEGTORAD(y) * 0.5f;
	float halfz = DEGTORAD(z) * 0.5f;

	float cosx = cosf(halfx);
	float cosy = cosf(halfy);
	float cosz = cosf(halfz);

	float sinx = sinf(halfx);
	float siny = sinf(halfy);
	float sinz = sinf(halfz);

	float cosycosz = cosy * cosz;
	float sinysinz = siny * sinz;

	w = cosx * cosycosz + sinx * sinysinz;
	v.x = sinx * cosycosz - cosx * sinysinz;
	v.y = cosx * siny * cosz + sinx * cosy * sinz;
	v.z = cosx * cosy * sinz - sinx * siny * cosz;

	Normalize();
}

void CQuaternion::QuatToAngleAxis(vector3f &axis, float &angle)
{
	float fSqrLen = v.LengthSquared();

	if (fSqrLen > 0.0f)
	{
		angle = RADTODEG(2.0f * acosf(w));
        float fAvoidDiv = 1.0f / sqrtf(fSqrLen);
        axis.x = v.x * fAvoidDiv;
        axis.y = v.y * fAvoidDiv;
        axis.z = v.z * fAvoidDiv;
    	}
	else
	{
		angle = 0.0f;
		axis.x = 0.0f;
		axis.y = 1.0f;
		axis.z = 0.0f;
	}
}

void CQuaternion::AngleAxisToQuat(vector3f axis, float angle)
{
	float fHalfAngle = 0.5f * angle;
	float fSin = sinf(fHalfAngle);
	w = cosf(fHalfAngle);
	v.x = fSin * axis.x;
	v.y = fSin * axis.y;
	v.z = fSin * axis.z;
}

void CQuaternion::QuatToRotationMatrix(matrix44f &rmatrix)
{
	float twox = v.x + v.x;
	float twoy = v.y + v.y;
	float twoz = v.z + v.z;

	float xx = twox * v.x;
	float yy = twoy * v.y;
	float zz = twoz * v.z;
	float wx = w * twox;
	float wy = w * twoy;
	float wz = w * twoz;
	float xy = v.x * twoy;
	float xz = v.x * twoz;
	float yz = v.y * twoz;

	rmatrix.m[0] = 1.0f - (yy + zz);
	rmatrix.m[1] = xy - wz;
	rmatrix.m[2] = xz + wy;
	rmatrix.m[3] = 0.0f;

	rmatrix.m[4] = xy + wz;
	rmatrix.m[5] = 1.0f - (xx + zz);
	rmatrix.m[6] = yz - wx;
	rmatrix.m[7] = 0.0f;

	rmatrix.m[8] =	xz - wy;
	rmatrix.m[9] =	yz + wx;
	rmatrix.m[10] =	1.0f - (xx + yy);
	rmatrix.m[11] = 0.0f;

	rmatrix.m[12] =	0.0f;
	rmatrix.m[13] = 0.0f;
	rmatrix.m[14] =	0.0f;
	rmatrix.m[15] = 1.0f;

	return;
}

//-------------------------------------------------------------------------
// overloaded operators
//-------------------------------------------------------------------------

CQuaternion CQuaternion::operator * (const CQuaternion q)
{
	// Note: Both quaternions must refer to the same coordinate axis!

	float w_ = w*q.w - v.x*q.v.x - v.y*q.v.y - v.z*q.v.z;
	float x_ = w*q.v.x + v.x*q.w + v.y*q.v.z - v.z*q.v.y;
	float y_ = w*q.v.y - v.x*q.v.z + v.y*q.w + v.z*q.v.x;
	float z_ = w*q.v.z + v.x*q.v.y - v.y*q.v.x + v.z*q.w;
	return CQuaternion(w_, x_, y_, z_);
}

CQuaternion CQuaternion::operator *= (const CQuaternion q)
{
	// Note: Both quaternions must refer to the same coordinate axis!

	float w_ = w*q.w - v.x*q.v.x - v.y*q.v.y - v.z*q.v.z;
	float x_ = w*q.v.x + v.x*q.w + v.y*q.v.z - v.z*q.v.y;
	float y_ = w*q.v.y - v.x*q.v.z + v.y*q.w + v.z*q.v.x;
	float z_ = w*q.v.z + v.x*q.v.y - v.y*q.v.x + v.z*q.w;

	w = w_;
	v.x = x_;
	v.y = y_;
	v.z = z_;

	return (*this);
}


void CQuaternion::operator = (const CQuaternion q)
{
	w = q.w;
	v.x = q.v.x;
	v.y = q.v.y;
	v.z = q.v.z;
}